#ifndef _DRIVER_CHASSIS_H
#define _DRIVER_CHASSIS_H
#include "pid.h"
#include "bsp_can.h"
#include "task_communicate.h"
#include "main.h"
#include "task_powerbuffer.h"


///****************普通步兵类型接口******************///

#define USE_SEPERATE_POWER 0 ///1使用分别计算k值的功率环

///****************普步底盘类型接口******************///
#define Mecanum_Chassis 0
#define Omni_Chassis 1
///***********************************************///
#define MaxSpeed_3508    9600
#define MaxCurrent_3508  16384
#define MaxEncoder_3508  8192
#define MaxCurrent_6020  30000
#define MaxSpeed_6020    320
#define MaxEncoder_6020  8192

#define infantryID 1 //大全向0 小全向1
#define USE_RANDOM_SPIN   1       //使用随机转速

///****************跟随零位初值设定******************///
#if infantryID == 0
#define CHASSIS_FOLLOW_INIT_VALUE 0.246094f//0.746094f-0.5f

///****************变功率飞坡******************///
#define MaxChangePower 280
#define MinChangePower 120
#define ChangePowerIncrease (0.1f)//线性
///***********************************************///
#define PIE (3.1415926f)

//*****************不同步兵PID参数*********************///
#define FOLLOW_OFFSET 0.05f
#define FOLLOW_OFFSET2 0.05f

#define	CHASSIS_MOTOR_KP	(10.0f)
#define	CHASSIS_MOTOR_KI	(0.0f)
#define	CHASSIS_MOTOR_KD	(0.0f)

#define CHASSIS_MOTOR_AP (6.5f)
#define CHASSIS_MOTOR_BP (0.0f)
#define CHASSIS_MOTOR_CP (0.0f)
#define CHASSIS_MOTOR_AI (0.0f)
#define CHASSIS_MOTOR_CI (0.0f)
#define CHASSIS_MOTOR_AD (0.0f)
#define CHASSIS_MOTOR_BD (0.0f)
#define CHASSIS_MOTOR_CD (0.0f)

#define CHASSIS_FOLLOW_KP	(3.0f)
#define CHASSIS_FOLLOW_KI	(0.0f)
#define CHASSIS_FOLLOW_KD	(0.7f)
#define CHASSIS_NOT_FOLLOW_TOLERANCE (0.0005f)
#define CHASSIS_FOLLOW_AP (3.0f)
#define CHASSIS_FOLLOW_BP (20.0f)
#define CHASSIS_FOLLOW_CP (10.0f)
#define CHASSIS_FOLLOW_AI (0.0f)
#define CHASSIS_FOLLOW_CI (0.0f)
#define CHASSIS_FOLLOW_AD (0.7f)
#define CHASSIS_FOLLOW_BD (1.0f)
#define CHASSIS_FOLLOW_CD (10.0f)

#elif infantryID==1
#define CHASSIS_FOLLOW_INIT_VALUE 0.500488f//0.000488f+0.5f//0.500488f-0.5f

///****************变功率飞坡******************///
#define MaxChangePower 250
#define MinChangePower 120
#define ChangePowerIncrease (0.3f)//线性
///***********************************************///
#define PIE (3.1415926f)

//*****************不同步兵PID参数*********************///
#define FOLLOW_OFFSET 0.05f
#define FOLLOW_OFFSET2 0.05f

#define	CHASSIS_MOTOR_KP	(10.0f)
#define	CHASSIS_MOTOR_KI	(0.0f)
#define	CHASSIS_MOTOR_KD	(0.0f)

#define CHASSIS_MOTOR_AP (6.5f)
#define CHASSIS_MOTOR_BP (0.0f)
#define CHASSIS_MOTOR_CP (0.0f)
#define CHASSIS_MOTOR_AI (0.0f)
#define CHASSIS_MOTOR_CI (0.0f)
#define CHASSIS_MOTOR_AD (0.0f)
#define CHASSIS_MOTOR_BD (0.0f)
#define CHASSIS_MOTOR_CD (0.0f)

#define CHASSIS_FOLLOW_KP	(3.0f)
#define CHASSIS_FOLLOW_KI	(0.0f)
#define CHASSIS_FOLLOW_KD	(0.7f)
#define CHASSIS_NOT_FOLLOW_TOLERANCE (0.0005f)
#define CHASSIS_FOLLOW_AP (3.0f)
#define CHASSIS_FOLLOW_BP (20.0f)
#define CHASSIS_FOLLOW_CP (10.0f)
#define CHASSIS_FOLLOW_AI (0.0f)
#define CHASSIS_FOLLOW_CI (0.0f)
#define CHASSIS_FOLLOW_AD (0.7f)
#define CHASSIS_FOLLOW_BD (1.0f)
#define CHASSIS_FOLLOW_CD (10.0f)
#endif
typedef struct
{
    float SetSpeedX;
    float SetSpeedY;
    float SpeedX;       //逆解算得到的X速度
    float lastSpeedX;
    float SpeedY;       //逆解算得到的Y速度
    float lastSpeedY;
    float Spin;

    float LastSetSpeedY;
    float LastSetSpeedX;

    PID PIDPower;
}ChassisSpeedMessegePort;

typedef struct
{
    float SetFollowValue;
    float FollowValue;
    PID PIDChassisFollow;
    float OffsetValue;
}ChassisFollow_TypeDef;


typedef struct
{
    struct{
        float	SetSpeed;
        float	Speed;
    }Speed;
    float precurrent  ;
    float ax ;
    float bx ;
    u8 CANMessege[8];
    PID PIDSpeed;

}ChassisMotorStruct;

typedef struct YawMotor
{
    struct{
        float SetLocation;
        float Location;
    }Location;
    struct					//电机速度相关值
    {
        float SetSpeed;
        float Speed;
    }Speed;
    u8 CANMessege[8];
}GimbalMotorStruct;

typedef struct {
    float Buffer;           //缓冲能量
    float Judge_Limit;      //裁判系统限制
    float NowPower;         //实际功率
    float LimitedPower_Now; //自我设定限制功率
    float powerall_ax ;     //功率环参数
    float powerall_bx ;     //功率环参数
#if USE_SEPERATE_POWER == 0
    float k;                //输出电流限制系数
    float kk;	            //I项系数     kk的偏大和偏小都会导致实际功率随此时电机转速变化而变化，无法稳定在定值
#elif USE_SEPERATE_POWER == 1
    float k[4];
    float kk[4];
#endif
    float R ;		        //I^2项系数   R越大功率阶跃效应越差  R越小精度会偏低
    float kp_Kincrease;     //调整kk的系数 使kk的数值能正好稳定在想要的功率 //原值是0.01
    float K_Power_Buffer;
    float upHill_k[4];
}ChassisPowerDataStruct;

typedef struct {
    float RemoteK;
    float RemoteIncreaseK;
    float PowerLimitSpinDefault;
}ChassisSpeedDataStruct;

enum  Status{
    StatusOn = 1,
    StatusOff= 0
};

//有关模式的标志变量
typedef struct
{
    uint8_t InitPower; //是1否0首次启动
    uint8_t USECALC2; //是1否0使用变结构PID
    uint8_t power_flag;
    uint8_t Use_AutoC;//是否使用自动电容
    uint8_t Use_RampC;//是否使用飞坡电容
    uint8_t RecoverState;//0表示电容没电了 1-4表示电量状态
}RoboStatu_Typedef;

extern GimbalMotorStruct   YawMotor;
extern ChassisMotorStruct  ChassisMotor[4];
extern ChassisSpeedMessegePort ChassisSpeed;
extern RoboStatu_Typedef Status;



void GetWheelMotorStatus(void);
void GetYawMotorStatus(void);
void ChassisFollowCalculate(ChassisSpeedMessegePort *Chassisspeed);
void PowerControlBySpeed(ChassisSpeedMessegePort *ChassisSpeed);
void ChassisPowerControl(void);
void ChassisInit(void);

void ChassisNewPowerControl(ChassisSpeedMessegePort ChassisSpeed);

void ChassisMotor_SpeedUpdate(ChassisSpeedMessegePort ChassisSpeed, float kx, float ky, float kspin);
void ChassisSetSpeed(float SpeedX,float SpeedY);
void ChassisCurrentSend(void);
void UseRemoteUpdate();

float GetPowerNow();
float GetPowerBuffer();
float GetPowerLimit();
void GetJudgePower(void);

#endif //_DRIVER_CHASSIS_H
